#include "motioncontrol.h"
#include "../VelocityPlan/velocityplan.h"
#include "../velocitymode/velocitymode.h"
#include "../list/listcnc.h"
#include <string.h>

typedef struct
{
	char type;
	MOTION_LINEARDATAINFO data;
	MOTION_INSINFO instruction;
}MOTION_NODE;

void *Motion_Plookheadlist = 0;


static int Motion_CheckOutputQueueIsFull()
{
	return 1;
}

void Motion_Controlthread()
{
	
	while (1)
	{

		//check output queue 
		while (!Motion_CheckOutputQueueIsFull())
		{
			
			//if output queue is not full			
			//fetch and put data into output queue
			
		}

	}

}






int Motion_ModifySpeed(void *Plist)
{	
	int id = Listcnc_GetEndNode(Plist);
	float s = 0;
	
	//because this is called only when we push a new node into the list
	//it definitely contains at least one data.
	id = Listcnc_GetEndNode(Plist);
	if(id == Listcnc_GetHeadNode(Plist))
	{
		//
		
	}
	else
	{
		
	}
		
		
	
	
	for (int i = 0; i < Listcnc_GetValidNodeNum(Plist); i++)
	{
		
		
		
	}
	return MOTION_OK;
}


int Motion_Push(void *Pnode, char type)
{
	MOTION_NODE work_node;
	int work_hr = MOTION_OK;

	switch (type)
	{
		case MOTION_DATA:
			work_node.type = MOTION_DATA;
			memcpy(&work_node.data, Pnode, sizeof(MOTION_LINEARDATAINFO));
			break;
		case MOTION_INS:
			work_node.type = MOTION_INS;
			memcpy(&work_node.instruction, Pnode, sizeof(MOTION_INSINFO));
			break;
		default:
			work_hr = MOTION_CMDTYPE_WRONG;
			break;
	}

	if (work_hr == MOTION_CMDTYPE_WRONG)
	{
		return MOTION_CMDTYPE_WRONG;
	}

	work_hr = Listcnc_Push(&work_node, Motion_Plookheadlist); // push node into list
	if (work_hr == LISTCNC_FULL)
	{
		return MOTION_WAITPUSH;
	}

	//run alorigthm on every node of the list
	Motion_ModifySpeed(Motion_Plookheadlist);

	return MOTION_OK;
}


